#include "broker.h"

#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>

struct bcshm_t shm;
struct bcshmseg_t shmseg;

void reader(){
  
  while(1){
    bcshm_read(FC_TLM, &shm, &shmseg.fc_tlm);

    printf("FC telemetry:\n");
    printf("\tflight mode: %u\n", shmseg.fc_tlm.flight_mode);
    printf("\tgps fix: %u\n", shmseg.fc_tlm.gps_fix);
    printf("\tmotors count: %d\n", shmseg.fc_tlm.actuators_count);
    for(int i = 0; i < shmseg.fc_tlm.actuators_count; ++i)
      printf("\t\tmotor %d : %d\n", i, shmseg.fc_tlm.actuators[i]);
    printf("\tAltitude: %f\n", shmseg.fc_tlm.hight);
    printf("\tVelocity: %f\n", shmseg.fc_tlm.speed);

    usleep(1e6 / 2);
  }
}

int main(int argc, char* argv[]) {

  bcshm_open(&shm);
  
  reader();
  
  bcshm_close(&shm);

  return 0;
}
